/*
 * Kinect Player
 * Kulikov Victor
 * kulikov.victor@gmail.com
 * GNU Lisence 2011
 */
#include "glkinectview.h"
#include <QtOpenGL>
#include <Kinect/kinectframe.h>

using namespace kinect;
GLKinectView::GLKinectView(QWidget *parent) :
    QGLWidget(parent)
{
}
void GLKinectView::initializeGL()
{
    qglClearColor(QColor::fromRgb(0,0,0,0));

    glGenTextures(1, &gl_rgb_tex);
    glBindTexture(GL_TEXTURE_2D, gl_rgb_tex);
    glShadeModel(GL_FLAT);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_CULL_FACE);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
    zoom = 0.3;
}

void GLKinectView::paintGL()
{
    if(0==_depth.data)
        return;
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glLoadIdentity();
    glTranslated(0.0, 0.0, 10.0);

    static unsigned int indices[480][640];
    static short xyz[480][640][3];
    int i,j;
    for (i = 0; i < 480; i++) {
        for (j = 0; j < 640; j++) {
            xyz[i][j][0] = j;
            xyz[i][j][1] = i;
            xyz[i][j][2] = _depth.at<ushort>(i,j);// [i*640+j];
            indices[i][j] = i*640+j;
        }
    }

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glLoadIdentity();

    glPushMatrix();
    glScalef(zoom,zoom,1);
    glTranslatef(0,0,-5.5);

    glRotated(xRot / 16.0, 1.0, 0.0, 0.0);
    glRotated(yRot / 16.0, 0.0, 1.0, 0.0);
    glRotated(zRot / 16.0, 0.0, 0.0, 1.0);

    glTranslatef(0,0,1.5);

    LoadVertexMatrix();

    // Set the projection from the XYZ to the texture image
    glMatrixMode(GL_TEXTURE);
    glLoadIdentity();

    glScalef(1/640.0f,1/480.0f,1);
    LoadRGBMatrix();
    LoadVertexMatrix();
    glMatrixMode(GL_MODELVIEW);

    glPointSize(1);

    glEnableClientState(GL_VERTEX_ARRAY);
    glVertexPointer(3, GL_SHORT, 0, xyz);
    glEnableClientState(GL_TEXTURE_COORD_ARRAY);
    glTexCoordPointer(3, GL_SHORT, 0, xyz);

    glEnable(GL_TEXTURE_2D);

    glBindTexture(GL_TEXTURE_2D, gl_rgb_tex);
    glTexImage2D(GL_TEXTURE_2D, 0, 3, 640, 480, 0, GL_RGB, GL_UNSIGNED_BYTE, _rgb.data);

    glPointSize(2.0f);
    glDrawElements(GL_POINTS, 640*480, GL_UNSIGNED_INT, indices);
    glPopMatrix();
    glDisable(GL_TEXTURE_2D);

}
void GLKinectView::mousePressEvent(QMouseEvent *event)
{
    lastPos = event->pos();
}

void GLKinectView::mouseMoveEvent(QMouseEvent *event)
{
    int dx = event->x() - lastPos.x();
    int dy = event->y() - lastPos.y();

    if (event->buttons() & Qt::LeftButton) {
        setXRotation(xRot + 8 * dy);
        setYRotation(yRot + 8 * dx);
    } else if (event->buttons() & Qt::RightButton) {
        setXRotation(xRot + 8 * dy);
        setZRotation(zRot + 8 * dx);
    }
    lastPos = event->pos();
}
void GLKinectView::wheelEvent(QWheelEvent * event)
{

    zoom+=(float)(event->delta())/1000;
    if(zoom<0.1)
        zoom=0.1;
    updateGL();
}
void GLKinectView::setXRotation(int angle)
{
    normalizeAngle(&angle);
         if (angle != xRot) {
             xRot = angle;
             //emit xRotationChanged(angle);
             updateGL();
         }
}

void GLKinectView::setYRotation(int angle)
{
    normalizeAngle(&angle);
        if (angle != yRot) {
            yRot = angle;
            //emit yRotationChanged(angle);
            updateGL();
        }
}

void GLKinectView::setZRotation(int angle)
{
    normalizeAngle(&angle);
        if (angle != zRot) {
            zRot = angle;
            //emit zRotationChanged(angle);
            updateGL();
        }
}
void GLKinectView::normalizeAngle(int *angle)
{
    while (*angle < 0)
             *angle += 360 * 16;
         while (*angle > 360 * 16)
             *angle -= 360 * 16;
}

void GLKinectView::reciveFrame(KinectFrame frame)
{
    _rgb = frame.rgb();
    _depth = frame.depth();
    this->update();
}
void GLKinectView::resizeGL(int width, int height)
{

    glViewport(0, 0, width, height);

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glOrtho(-0.5, +0.5, -0.5, 0.5, 4.0, 15.0);
    glMatrixMode(GL_MODELVIEW);
}
void GLKinectView::LoadVertexMatrix()
{
    //Correction for allignement may depend of kinect device!
    float fx = 589.54f;
    float fy = 584.22f;
    float a = -0.0030711f;
    float b = 3.3309495f;
    float cx = 296.96f;
    float cy = 258.66f;
    GLfloat mat[16] = {
        1/fx,     0,  0, 0,
        0,    -1/fy,  0, 0,
        0,       0,  0, a,
        -cx/fx, cy/fy, -1, b
    };
    glMultMatrixf(mat);
}
void GLKinectView::LoadRGBMatrix()
{
    float mat[16] = {
        5.34866271e+02,   3.89654806e+00,   0.00000000e+00,   1.74704200e-02,
        -4.70724694e+00,  -5.28843603e+02,   0.00000000e+00,  -1.22753400e-02,
        -3.19670762e+02,  -2.60999685e+02,   0.00000000e+00,  -9.99772000e-01,
        -6.98445586e+00,   3.31139785e+00,   0.00000000e+00,   1.09167360e-02
    };
    glMultMatrixf(mat);
}
